/*******************************************************************************
 * Copyright 2009 DCSpectrometer - http://code.google.com/p/dcspectrometer 
 *  
 * Licensed under the Apache License, Version 2.0 (the "License"); 
 * you may not use this file except in compliance with the License. 
 * You may obtain a copy of the License at 
 *  
 *     http://www.apache.org/licenses/LICENSE-2.0 
 *     
 * Unless required by applicable law or agreed to in writing, software 
 * distributed under the License is distributed on an "AS IS" BASIS, 
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
 * See the License for the specific language governing permissions and 
 * limitations under the License. 
 *******************************************************************************/
package com.dcspectrometer;

import java.io.BufferedReader;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.StringTokenizer;

import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.ArrayRealVector;
import org.apache.commons.math.linear.DecompositionSolver;
import org.apache.commons.math.linear.LUDecompositionImpl;
import org.apache.commons.math.linear.RealMatrix;
import org.apache.commons.math.linear.RealVector;
import org.apache.commons.math.linear.SingularValueDecompositionImpl;

/** Represents sensor RGB */
public class SensorResponseCurve {
  public static final int RED_CHANNEL = 0;
  public static final int GREEN_CHANNEL = 1;
  public static final int BLUE_CHANNEL = 2;
  public static final double MIN_SPECTROMETER_READING = 10;
  private static int NUMBER_OF_WAVELENGTH_BANDS = 50;

  private final int CHANNELS = 3;
  private String[] _ledNames;
  private int channelsData[][];
  private HashMap<String, double[]> _ledSpectra;
  private double[][] responseCurve;
  private static double LAMBDA = 1;

  // TODO: this is ASS Backwards. Correct later
  private static int firstWavelengthIndex = 0;
  private static double wavelengthStepSize = 0;

  public SensorResponseCurve(HashMap<String, int[]> ledMapping, String spectraFileName)
      throws IllegalArgumentException, FileNotFoundException, IOException {
    if (ledMapping == null) {
      throw new IllegalArgumentException("Wavelegth mapping is null.");
    }

    if (ledMapping.size() < 2) {
      throw new IllegalArgumentException("Wavelength mapping must have at least 2 data points.");
    }

    String keys[] = ledMapping.keySet().toArray(new String[0]);
    Arrays.sort(keys);
    _ledNames = new String[keys.length];
    channelsData = new int[keys.length][CHANNELS];

    for (int i = 0; i < keys.length; i++) {
      _ledNames[i] = keys[i];
      channelsData[i] = ledMapping.get(keys[i]);
    }

    _ledSpectra = loadLEDSpectrometerMeasurements(spectraFileName);

    // printSpectra(_ledSpectra, _ledNames);

    responseCurve = new double[CHANNELS][_ledSpectra.get(ImageAnalyser.WAVELENGTH_COLUMN_NAME).length];

    double[][] A = constructA(_ledSpectra, _ledNames, NUMBER_OF_WAVELENGTH_BANDS);

    printMatrix(A, _ledNames.length);

    for (int i = 0; i < CHANNELS; i++) {
      double[] b = constructb(ledMapping, _ledNames, i, NUMBER_OF_WAVELENGTH_BANDS);

      printVector(b, "b" + i);

      responseCurve[i] = solveAxb(A, b);

      printVector(responseCurve[i], "response" + i + "l" + (int) LAMBDA);

    }

    double[] pickedWavelengths = new double[responseCurve[0].length];
    for (int i = 0; i < responseCurve[0].length; i++) {
      pickedWavelengths[i] = _ledSpectra.get(ImageAnalyser.WAVELENGTH_COLUMN_NAME)[firstWavelengthIndex
          + (int) (i * wavelengthStepSize)];
    }
    printVector(pickedWavelengths, "wl");

  }

  public static void printSpectra(HashMap<String, double[]> ledSpectra, String[] ledNames) {
    /** Print headers */
    System.out.println();
    System.out.print(ImageAnalyser.WAVELENGTH_COLUMN_NAME + "," + ImageAnalyser.DARK_COLUMN_NAME);
    for (int i = 0; i < ledNames.length; i++) {
      System.out.print("," + ledNames[i]);
    }
    System.out.println();

    /** Print Values */
    for (int i = 0; i < ledSpectra.get(ledNames[0]).length; i += 20) {
      System.out.print(String.format("%f,%f",
          ledSpectra.get(ImageAnalyser.WAVELENGTH_COLUMN_NAME)[i], ledSpectra
              .get(ImageAnalyser.DARK_COLUMN_NAME)[i]));
      for (int j = 0; j < ledNames.length; j++) {
        System.out.print(String.format(",%f", ledSpectra.get(ledNames[j])[i]));
      }
      System.out.println();
    }
  }

  public static void printVector(double[] vector, String name) {
    System.out.println();
    System.out.print(name + " = [");
    for (int j = 0; j < vector.length; j++) {
      System.out.print(vector[j] + " ");
      if ((j + 1) < vector.length) {
        System.out.print(";");
      }
    }
    System.out.println("];");
  }

  public static void printMatrix(double[][] matrix, int rows) {
    System.out.println();
    if (rows < 1) {
      rows = matrix.length;
    }
    System.out.print("A = [");
    for (int i = 0; i < rows; i++) {
      System.out.print("[");
      for (int j = 0; j < matrix[0].length; j++) {
        System.out.print(matrix[i][j] + " ");
      }
      System.out.print("]");
      if ((i + 1) < rows) {
        System.out.print(";");
      }
    }
    System.out.println("];");
  }

  public String toString() {
    String value = "";

    for (int i = 0; i < _ledNames.length; i++) {
      value += "LED = " + _ledNames[i] + ", RGB = " + "(" + channelsData[i][RED_CHANNEL] + ", "
          + channelsData[i][GREEN_CHANNEL] + ", " + channelsData[i][BLUE_CHANNEL] + ")\n";
    }

    double[] wavelengths = _ledSpectra.get(ImageAnalyser.WAVELENGTH_COLUMN_NAME);
    double[] pickedWavelengths = new double[responseCurve[0].length];
    for (int i = 0; i < responseCurve[0].length; i++) {
      value += "wavelength = " + wavelengths[firstWavelengthIndex + (int) (i * wavelengthStepSize)]
          + ", RGB = " + "(" + responseCurve[RED_CHANNEL][i] + ", "
          + responseCurve[GREEN_CHANNEL][i] + ", " + responseCurve[BLUE_CHANNEL][i] + ")\n";
      pickedWavelengths[i] = wavelengths[firstWavelengthIndex + (int) (i * wavelengthStepSize)];
    }

    printVector(pickedWavelengths, "wl");

    return value;
  }

  public static HashMap<String, double[]> loadLEDSpectrometerMeasurements(String spectraFileName)
      throws FileNotFoundException, IOException {
    ArrayList<String> columnHeaders = new ArrayList<String>();
    ArrayList<Double>[] columnValues = null;

    /** Read in data. */
    File file = new File(spectraFileName);

    BufferedReader bufferReader = new BufferedReader(new FileReader(file));
    String nextLine = null;
    int row = 0;
    int col;

    while ((nextLine = bufferReader.readLine()) != null) {
      StringTokenizer stringTokenizer = new StringTokenizer(nextLine, ",");
      col = 0;

      if (row == 1) {
        columnValues = new ArrayList[columnHeaders.size()];
      }

      while (stringTokenizer.hasMoreTokens()) {
        try {
          if (row == 0) {
            columnHeaders.add(stringTokenizer.nextToken());
          } else {
            if (row == 1) {
              columnValues[col] = new ArrayList<Double>();
            }
            columnValues[col].add(new Double(stringTokenizer.nextToken()));
          }

          col++;
        } catch (IndexOutOfBoundsException e) {
          throw new IOException("Incorrect file format.");
        }
      }
      row++;
    }

    bufferReader.close();

    /** Make sure data sizes are appropriate. */
    if (columnHeaders.size() != columnValues.length) {
      throw new IOException("Incorrect file format.");
    }

    for (int i = 0; i < columnHeaders.size(); i++) {
      if (columnValues[i].size() != columnValues[0].size()) {
        throw new IOException("Incorrect file format.");
      }
    }

    /** Construct HashMap */
    HashMap<String, double[]> spectra = new HashMap<String, double[]>();
    double[] nextValueList = null;
    for (int i = 0; i < columnHeaders.size(); i++) {
      nextValueList = new double[columnValues[0].size()];

      for (int j = 0; j < columnValues[0].size(); j++) {
        nextValueList[j] = columnValues[i].get(j);
      }

      spectra.put(columnHeaders.get(i), nextValueList);
    }

    return spectra;
  }

  public static double[][] constructA(HashMap<String, double[]> spectra, String[] ledNames,
      int numberOfWavelengthCategories) {
    int numberOfLEDs = ledNames.length;
    int numberOfSampledWavelength = spectra.get(ImageAnalyser.WAVELENGTH_COLUMN_NAME).length;

    if (numberOfWavelengthCategories < 1) {
      numberOfWavelengthCategories = numberOfSampledWavelength;
    }

    double[][] A = new double[numberOfWavelengthCategories][numberOfWavelengthCategories];
    double[] darkResponse = spectra.get(ImageAnalyser.DARK_COLUMN_NAME);

    /** Find index of minimum wavelength with significant response */
    int minIndex = 0;
    for (int i = 0; i < numberOfSampledWavelength; i++) {
      for (int j = 0; j < numberOfLEDs; j++) {
        if (spectra.get(ledNames[j])[i] > (darkResponse[i] + MIN_SPECTROMETER_READING)) {
          minIndex = i;
          break;
        }
      }
      if (minIndex != 0) {
        break;
      }
    }

    firstWavelengthIndex = minIndex;

    /** Find index of maximum wavelength with significant response */
    int maxIndex = numberOfSampledWavelength - 1;
    for (int i = numberOfSampledWavelength - 1; i >= 0; i--) {
      for (int j = 0; j < numberOfLEDs; j++) {
        if (spectra.get(ledNames[j])[i] > (darkResponse[i] + MIN_SPECTROMETER_READING)) {
          maxIndex = i;
          break;
        }
      }
      if (maxIndex != (numberOfSampledWavelength - 1)) {
        break;
      }
    }

    int categoryWidth = (maxIndex - minIndex) / numberOfWavelengthCategories;

    wavelengthStepSize = categoryWidth;

    for (int i = 0; i < numberOfLEDs; i++) {
      double[] ledResponse = spectra.get(ledNames[i]);
      for (int j = 0; j < numberOfWavelengthCategories; j++) {
        // TODO: Switch to averaging later.
        double value = sumOfArrayElements(ledResponse, minIndex + (int) (j * categoryWidth),
            categoryWidth)
            - sumOfArrayElements(darkResponse,minIndex + (int) (j * categoryWidth), categoryWidth);
        A[i][j] = ((value > 0) ? value : 0);
      }
    }

    for (int i = numberOfLEDs; i < numberOfWavelengthCategories; i++) {
      A[i][i - numberOfLEDs] = +LAMBDA;
      // if ((i + 1) < numberOfWavelengthCategories) {
      A[i][i - numberOfLEDs + 1] = -LAMBDA;
      // } else {
      // A[i][0] = -LAMBDA;
      // }
    }

    return A;
  }

  private static double sumOfArrayElements(double[] array, int startIndex, int length) {
    double sumOfElements = 0;

    for (int i = 0; i < length; i++) {
      sumOfElements += array[startIndex + i];
    }

    return sumOfElements;
  }

  public static double[] constructb(HashMap<String, int[]> ledMapping, String[] ledNames,
      int channel, int numberOfWavelengthCategories) {
    int numberOfLEDs = ledNames.length;
    int numberOfSampledWavelength = numberOfWavelengthCategories;
    double[] b = new double[numberOfSampledWavelength];

    for (int i = 0; i < numberOfLEDs; i++) {
      b[i] = ledMapping.get(ledNames[i])[channel];
    }

    for (int i = numberOfLEDs; i < numberOfSampledWavelength; i++) {
      b[i] = 0;
    }

    return b;
  }

  public static double[] solveAxb(double[][] A, double[] b) {
    RealMatrix matrix = new Array2DRowRealMatrix(A);

    LUDecompositionImpl lu = new LUDecompositionImpl(matrix);
    DecompositionSolver solver = lu.getSolver();

    RealMatrix aInv = solver.getInverse();
    // RealMatrix vector = new Array2DRowRealMatrix(b);
    double[] solutions = aInv.preMultiply(b);

    /*
     * System.out.print("Answers: {" + solutions[0]); for (int i = 1; i < solutions.length; i++) {
     * System.out.print(", " + solutions[i]); } System.out.println("}");
     */

    return solutions;
  }
}
